#include "Action.h"

//смена режима
//================================================================================
void util_mode(char _mode)
	{
	if(_mode>=AMOUNT_MODE)	//если такого режима не существует 
		{
		Mode=0;
		}
	else
		{
		Mode=_mode;
		}
	
	//сброс переменных и счетчиков
	drive_counter=0;
	bumper_counter=0;
	
	drive_time=0;
	led_time=0;
	
	}
//================================================================================




//управление циклом работы
//================================================================================
void cycle()	
	{
	if(cycle_time>0)		//если время не пришло
		{
		return;
		}
		
	 switch(cycle_counter)
		{
		//режим ожидания
		///////////////////////////////	
		case 0:			
		if(Mode>0)
			{
			util_mode(WAITING);
			cycle_rep=0;	//счетчик повторений
			}
		return;
		///////////////////////////////	
		
		//квадратная спираль
		///////////////////////////////	
		case 1:			
		if(cycle_rep>=2)
			{
			cycle_counter=0;
			return;
			}
				
		util_mode(SPIRAL);	
		cycle_counter++;			//на следующий режим 
		cycle_time=60000;			//через минуту
		Timer_Task(&cycle_time);	
		return;
		///////////////////////////////
		
		//вдоль стены
		///////////////////////////////	
		case 2:			
		util_mode(ALONG);	
		
		cycle_counter++;			//на следующий режим 
		cycle_time=60000;			//через минуту
		Timer_Task(&cycle_time);	
		return;
		///////////////////////////////	
		
		///////////////////////////////	
		case 3:
		util_mode(MOWER);				//"газонокосилка"
		
		cycle_counter=1;			//в начало 
		cycle_rep++;
		cycle_time=60000;			//через минуту
		Timer_Task(&cycle_time);
		return; 
		///////////////////////////////	 

		} 
		
	}
//================================================================================
	
	
//индикатор режима работы
//================================================================================
void led()
	{
	if(led_time!=0)		//если время не пришло
		{
		return;
		}
		
	if(Mode>0)			//если уборка
		{
		if(PORTC & (1<<5))
			{
			PORTC &= ~(1<<5);
			
			led_time=300;	//тушим на 0.3 сек 
			Timer_Task(&led_time);
			}
		else
			{
			PORTC |= (1<<5);
			
			led_time=200;	//включаем на 0.2 сек
			Timer_Task(&led_time);
			}
		}
	else			//если режим ожидания
		{
		if(PORTC & (1<<5))
			{
			PORTC &= ~(1<<5);
			
			led_time=1000;
			Timer_Task(&led_time);	//тушим на 2.5 сек 
			}
		else
			{
			PORTC |= (1<<5);
			
			led_time=1000;
			Timer_Task(&led_time);	//включаем на 1 сек
			}
		}
	}
//================================================================================
	
	
//Управляющая кнопка
//================================================================================
void button()
	{
	if(button_time!=0)		//если время не пришло
		{
		return;
		}
		
	switch(button_counter)
		{
		//проверяем наличие сигнала от кнопки
		///////////////////////////////	
		case 0:
		if(PIND & (1<<PIND3))				//если сейчас кнопка нажата
			{
			if(button_flag==0)				//если в прошлый раз кнопка была опущена
				{
				button_flag=1;				//запоминаем что сейчас кнопка нажата
				
				button_time = 300;
				Timer_Task(&button_time);	//проверить наличие сигнала через 3/10 секунды
				button_counter++;
				}
			}
		else
			{
			button_flag=0;					//запоминаем что сейчас кнопка опущена
			}
		return;	
		///////////////////////////////
		
		
		//подтверждаем сигнал на кнопке
		/////////////////////////////// 
		case 1:
		if(PIND & (1<<PIND3))				//если сигнал от кнопки подтвержден
			{
			if(cycle_counter==0)			//если сейчас режим ожидания
				{
				cycle_counter=1;			//переходим к режиму уборки
				}
			else 							//если сейчас режим уборки
				{
				cycle_time=0;				//прерываем режим уборки
				cycle_counter=0;			//в режим ожидания
				}
			}
		else 								//если сигнал не подтвержден 	
			{
			button_flag=0;					//запоминаем что сейчас кнопка опущена
			}
		button_counter=0;					//GOTO 0
		///////////////////////////////
		}
	}
//================================================================================
	
	
//бампер
//================================================================================
void bumper()
	{
	/*выбираем режим*/
	
	//режим ожидания
	if(Mode==WAITING)
		{
		return;
		}
	
	//режим движение вдоль стены	
	if(Mode==ALONG)
		{
		switch(bumper_counter)
			{
			//поиск стены
			///////////////////////////////
			case 0:		
			if((PIND & (1<<PIND0)) | (PIND & (1<<PIND1)))	//если стена найдена
				{
				bumper_counter=1;			//на следующий шаг
				drive_counter=1;			//
				}
			break;
			///////////////////////////////
			
			//удерживание у стены
			///////////////////////////////
			case 1:		
			if((PIND & (1<<PIND0)) | (PIND & (1<<PIND1)))	//если есть препятствие
				{
				drive_counter=2;		//уклонение
				}
			break;
			///////////////////////////////
			}
		return;
		}
	
	
	//квадратная спираль
	if(Mode==SPIRAL)
		{
		if((PIND & (1<<PIND0)) | (PIND & (1<<PIND1)))	//если есть препятствие
			{
			//заканчиваем спираль
			cycle_time=0;								//на следующий режим
			}
			
		return;
		}
	
	//газонокосилка
	if(Mode==MOWER)
		{
		if((PIND & (1<<PIND0)) | (PIND & (1<<PIND1)))	//если есть препятствие
			{
			drive_counter=2;		//уклонение
			} 
			
		return;
		}
					
	}
//================================================================================
	
	
//алгоритм привода
//================================================================================
void drive()
	{
	/*выбираем режим*/
	
	
	//режим ожидания
	if(Mode==WAITING)
		{
		M_drive(0, 0, 0, 0);	//стоп
		return;
		}
	
	//режим движение вдоль стены
	if(Mode==ALONG)
		{
		switch(drive_counter) 
			{
			//поиск стены
			///////////////////////////////
			case 0:		
			M_drive(1, 10, 1, 10);	//вперед
			
			drive_speed=7;			//начальная скорость заворота 
			drive_time=250;			//увеличение угла заворота каждые 1/4 сек
			Timer_Task(&drive_time);
			break;
			///////////////////////////////
			
			//удерживание у стены
			///////////////////////////////
			case 1:		
			if(drive_time==0)		//постепенно уменьшаем скорость правого колеса
				{
				if(drive_speed!=0)
					{
					drive_speed--;
					drive_time=250;
					Timer_Task(&drive_time);
					}
				}
				
			M_drive(1, 10, 1, drive_speed);		//заворот направо
			break;
			
			///////////////////////////////
			
			//уклонение
			///////////////////////////////
			case 2:		
			M_drive(-1, 8, 1, 8);		//разворот на месте влево
			
			drive_speed=7;
			drive_time=250;
			Timer_Task(&drive_time);
			
			drive_counter=1;		//GOTO 1
			break;
			}
			///////////////////////////////
			
		return;
		}
		
	//квадратная спираль
	if(Mode==SPIRAL)
		{
		if(drive_time>0)		//если не пришло время
			{
			return;
			}
			
		switch(drive_counter)
			{
			///////////////////////////////
			case 0:
			M_drive(1, 10, 1, 10);			//вперед	
			drive_time=way; 			 	//в течении way сек
			Timer_Task(&drive_time);
			
			drive_counter++;				//к следующему шагу
			break;
			///////////////////////////////
			
			///////////////////////////////
			case 1:
			M_drive(1, 0, 1, 10);			//заворот влево 
			drive_time=WRAP_90;				//на 90 град
			Timer_Task(&drive_time);
			
			drive_counter++;				//к следующему шагу
			break;
			///////////////////////////////
			
			///////////////////////////////
			case 2:
			M_drive(1, 10, 1, 10);			//вперед	
			drive_time=way;  				//в течении way сек
			Timer_Task(&drive_time);
			
			drive_counter++;				//к следующему шагу
			break;
			///////////////////////////////
			
			///////////////////////////////
			case 3:
			M_drive(1, 0, 1, 10);			//заворот влево 
			drive_time=WRAP_90;				//на 90 град
			Timer_Task(&drive_time);
			
			way+=500;	//увеличиваем сторону спирали
			
			drive_counter=0;				//GOTO 0
			break;
			///////////////////////////////
			}
			
		return;
		}
	
	//"газонокосилка"
	if(Mode==MOWER)
		{
	
		switch(drive_counter)
			{
			//вперед
			///////////////////////////////
			case 0:					
			if(drive_time>0)		//если не пришло время
				{
				return;
				}
				
			M_drive(1, 10, 1, 10);	//up
			
			drive_counter=1;		//к следующему шагу
			break;
			///////////////////////////////
			
			
			//считаем пройденный путь
			///////////////////////////////
			case 1:					
			if(drive_time>0)		//если не пришло время
				{
				return;
				}
				
			way++;					//засчитываем путь
			drive_time=1000;		//каждую  секунду
			Timer_Task(&drive_time);
			break;
			///////////////////////////////
			
			
			///////////////////////////////
			case 2:					//отъезд назад
			M_drive(-1, 10, -1, 10);	
			
			drive_counter=3;		//к следующему шагу
			break;
			///////////////////////////////
			
			///////////////////////////////
			case 3:					//поворот от стены
			
			if(tumbling_flag==0)			//если  в прошлый раз поворачивали направо
				{
				M_drive(-1, 10, 1, 10);		//поворачиваем налево
				tumbling_flag=1;
				}
			else 							//если в прошлый раз поворачивали налево
				{
				M_drive(1, 10, -1, 10);		//поворачиваем направо
				tumbling_flag=0;
				}
					
			if(way<=way_s/2)				//если пройденный путь в 2 раза меньше чем путь пройденный в прошлый раз
				{
				drive_time=TURN_90;			//на 90 град 
				}
			else 							//иначе
				{
				drive_time=TURN_160;		//на 160 град
				}
			Timer_Task(&drive_time);
					
			way_s=way;						//запоминаем пройденный в этот раз путь
			way=0;							//начинаем считать заново
			
			drive_counter=0;				//GOTO 0
			break;
			///////////////////////////////
			
			}

		return;
		}
	}
//================================================================================	



//уборочный узел
//================================================================================	
void cleaner()
	{
	if(Mode==0)		//если режим ожидания 
		{
		OFF_Cleaner;
		}
	else
		{
		ON_Cleaner;
		}
	}
//================================================================================	
		
